#include "matrix.h"
#include <math.h>

#define cos cosf
#define sin sinf

void translateMatrixFPU(float m[4][4], float vec[3], float mout[4][4])
{
    float m2[4][4] = { 
        {1.0f, 0.0f, 0.0f, vec[0]},
        {0.0f, 1.0f, 0.0f, vec[1]},
        {0.0f, 0.0f, 1.0f, vec[2]},
        {0.0f, 0.0f, 0.0f, 1.0f}
    };

    multMatrix44FPU(m,m2,mout);
}

void rotateMatrixXFPU(float m[4][4], float ang, float mout[4][4], bool r)
{
    float s = sin(ang);
    float c = cos(ang);

    float m2[4][4] = { 
        {1.0f, 0.0f, 0.0f, 0.0f},
        {0.0f,    c,   -s, 0.0f},
        {0.0f,    s,    c, 0.0f},
        {0.0f, 0.0f, 0.0f, 1.0f}
    };

    if(!r) multMatrix44FPU(m,m2,mout);
    else multMatrix44FPU(m2,m,mout);
}

void rotateMatrixYFPU(float m[4][4], float ang, float mout[4][4], bool r)
{
    float s = sin(ang);
    float c = cos(ang);

    float m2[4][4] = { 
        {   c, 0.0f,    s, 0.0f},
        {0.0f, 1.0f, 0.0f, 0.0f},
        {  -s, 0.0f,    c, 0.0f},
        {0.0f, 0.0f, 0.0f, 1.0f}
    };

    if(!r) multMatrix44FPU(m,m2,mout);
    else multMatrix44FPU(m2,m,mout);
}

void rotateMatrixZFPU(float m[4][4], float ang, float mout[4][4], bool r)
{
    float s = sin(ang);
    float c = cos(ang);

    float m2[4][4] = { 
        {   c,   -s, 0.0f, 0.0f},
        {   s,    c, 0.0f, 0.0f},
        {0.0f, 0.0f, 1.0f, 0.0f},
        {0.0f, 0.0f, 0.0f, 1.0f}
    };

    if(!r) multMatrix44FPU(m,m2,mout);
    else multMatrix44FPU(m2,m,mout);
}

